#include "../globals/globals.h"
#include "../pid/pid.h"
#include "auto_lib.h"

bool arc_turn_pid_adjust_force_stop = false;
bool arc_turn_pid_adjust_running = false;

struct Arg_arc_turn_pid_adjust_agl_absolute {
  bool run_done;  // 状态标志，是否完成
  double aim_agl;
  double max_pwr;
  int timeout;
  bool waitForCompletion;
  double fwd_pwr;
} arg_arc_turn_pid_adjust_agl_absolute;
//////////////////////////////////////////////////////////////////////////////
void arc_turn_pid_adjust_stop() {
  arc_turn_pid_adjust_force_stop = true;
  while_wait(arc_turn_pid_adjust_running);
  arc_turn_pid_adjust_force_stop = false;
}
bool is_arc_turn_pid_adjust_done() {
  return arg_arc_turn_pid_adjust_agl_absolute.run_done;
}
///////////////////////////////////////////////////////////////////////////////
/*
点动
*/
///////////////////////////////////////////////////////////////////////////////
void arc_turn_point_move_absolute(double agl_relative, double fwd_brake_pwr,
                                  int timeout = 100) {
  chassis.FT_set_voltage(fwd_brake_pwr, sgn(agl_relative) * 17.5);
  double agl_aim_abs = get_gyro() + agl_relative;
  timer t;
  while (1) {
    if (sgn(agl_relative) == 1)  // froward
      if (get_gyro() > agl_aim_abs - 0.08) {
        // COUT_TURN << "pmove: turn ok: " << get_gyro() << endl;
        break;
      }
    if (sgn(agl_relative) == -1)  // backward
      if (get_gyro() < agl_aim_abs + 0.08) {
        // COUT_TURN << "pmove: backward ok: " << get_gyro() << endl;
        break;
      }

    if (t.time() > timeout) {
      // COUT_TURN << "pmve: timeout\n";
      break;
    }
    wait(2);
  }

  chassis.mtr_stop(brakeType::brake);
}

///////////////////////////////////////////////////////////////////////////////
/*
启动函数
*/
///////////////////////////////////////////////////////////////////////////////
void arc_turn_agl_absolute(double aim_agl, double fwd_pwr, double max_pwr,
                           int timeout, bool waitForCompletion) {
  // 等待前一個线程结束
  while (arc_turn_pid_adjust_running) wait(5);

  if (abs(max_pwr) + abs(fwd_pwr) > 100) max_pwr = 100 - abs(fwd_pwr);

  arg_arc_turn_pid_adjust_agl_absolute = {false,   aim_agl,           max_pwr,
                                          timeout, waitForCompletion, fwd_pwr};

  arc_turn_pid_adjust_force_stop = false;

  // 建立线程
  void arc_turn_absolute_cb_static();
  thread t(arc_turn_absolute_cb_static);

  // 等待启动
  while (!arc_turn_pid_adjust_running) wait(5);

  // 等待完成
  if (waitForCompletion) {
    while (arc_turn_pid_adjust_running && !arc_turn_pid_adjust_force_stop)
      wait(20);
  }
}
///////////////////////////////////////////////////////////////////////////////
/*
线程执行函数
*/
///////////////////////////////////////////////////////////////////////////////
void arc_turn_absolute_cb_static() {
  // 建立局部变量，供本线程使用
  double aim_agl = arg_arc_turn_pid_adjust_agl_absolute.aim_agl;
  double max_pwr = arg_arc_turn_pid_adjust_agl_absolute.max_pwr;
  int timeout = arg_arc_turn_pid_adjust_agl_absolute.timeout;
  double fwd_pwr = arg_arc_turn_pid_adjust_agl_absolute.fwd_pwr;

  arc_turn_pid_adjust_running = true;
  wait(10);  // cpu shift

  if (timeout == 0) timeout = 1000000;  // long time enough

  ////////////////////////////////////////////////////////////////////////////////////
  timer t;
  timer t_run;
  int begin_dir = sgn(aim_agl - get_gyro());

  double begin_angle;
  double aim_angle = aim_agl;

#define agl_abs_left abs(aim_angle - get_gyro())

  double spd_abs = 20;

  begin_angle = get_gyro();

  double agl_all_abs = abs(aim_angle - begin_angle);
  pid_ctrl pid_angle = pid_ctrl::get_instance_old_version(
      1.2, 0.05, 0, 20, aim_angle, 0.3, 5, max_pwr);

  double output_turn = 0;
  double agl_err;

  // 2: even speed close to aim
  while (!arc_turn_pid_adjust_force_stop && t_run.time() < timeout &&
         agl_abs_left > 8 && agl_abs_left > agl_all_abs * 0.08) {
    // turn
    agl_err = aim_angle - get_gyro();
    // over run
    if (sgn(begin_dir) != sgn(agl_err)) break;

    output_turn = pid_angle.pid_output_need_err(agl_err);
    if (abs(output_turn) < 10) output_turn = sgn(output_turn) * 10;

    // output all
    chassis.LR_set_voltage(output_turn + fwd_pwr, -output_turn + fwd_pwr);
    wait(2);
  }

  // 3: slow down
  spd_abs = abs(output_turn);
  while (!arc_turn_pid_adjust_force_stop && t_run.time() < timeout &&
         agl_abs_left > 1.5 && agl_abs_left > agl_all_abs * 0.02) {
    chassis.LR_set_voltage(begin_dir * spd_abs + fwd_pwr,
                          -begin_dir * spd_abs + fwd_pwr);
    // over run
    agl_err = aim_angle - get_gyro();
    if (sgn(begin_dir) != sgn(agl_err)) break;

    spd_abs--;
    if (spd_abs < 15) {
      if (agl_abs_left < 3) {
        if (spd_abs <= 2) {
          spd_abs = 2;
          break;
        } else
          spd_abs -= 2;
      } else  // left too much
        spd_abs = 15;
    }
    wait(2);
  }
  // 4: point move to aim
  int pk = 0;
  bool pmove_err_ok = false;
  while (!arc_turn_pid_adjust_force_stop && t_run.time() < timeout) {
    agl_err = aim_angle - get_gyro();

    if (abs(agl_err) > 3)
      arc_turn_point_move_absolute(0.1 * sgn(agl_err), -sgn(fwd_pwr) * 5);
    else
      arc_turn_point_move_absolute(0.2 * sgn(agl_err), -sgn(fwd_pwr) * 5);
    if (abs(agl_err) < 0.2 && abs(get_gyro_rate()) < 25) {
      pmove_err_ok = true;
      break;
    }
    pk++;
    wait(2);
  }
  // 6: chassis.stop
  chassis.mtr_stop(brakeType::brake);
  arg_arc_turn_pid_adjust_agl_absolute.run_done = true;
  arc_turn_pid_adjust_running = false;
}
